package roboSoccer;

/**
 * The FieldObject class represents a physical object on the field. This
 * includes the ball and the robots, although the robots are actually
 * represented with the class RoboPlayer which extends this class.
 * <p>
 * Physical characteristics are held in this class. The position, velocity, and
 * size of the object are stored here. The position can be updated with the
 * update method, which also estimates the velocity based on past positions.
 * <p>
 * The class has some other methods to get useful information about the object.
 * 
 * @author Ben Hopkins
 * 
 */
public class FieldObject {


	/**
	 * Size in pixels of the object.
	 */
	public static final int sizeRadius = 3;

	/**
	 * Position of the object on the field, from the top left.
	 */
	public Vector position;
	// and a velocity
	/**
	 * Estimated velocity of the object is calculated from the last few position
	 * values.
	 */
	public Vector velocity;

	// how many former positions we will store
	private final int storePast = 10;
	private int cDex; // current index at the head of this array
	// the stored former positions
	private Vector[] pP;

	/**
	 * Default Constructor initializes all variables to default values.
	 */
	public FieldObject() {
		position = new Vector(100, 100);
		velocity = new Vector();
		pP = new Vector[storePast];
		for (int i = 0; i < storePast; i++)
			pP[i] = new Vector();
		cDex = 0;
	}

	/**
	 * This constructor allows the position to be set.
	 * 
	 * @param position
	 *            A Vector for the position of the object.
	 */
	public FieldObject(Vector position) {
		this.position = position;
		velocity = new Vector();
		pP = new Vector[storePast];
		for (int i = 0; i < storePast; i++)
			pP[i] = new Vector();
		cDex = 0;
	}

	/**
	 * Updates the position of the field object with a new position. The newest
	 * velocity is also calculated in this method.
	 * 
	 * @param position
	 *            The Vector position that the object now occupies.
	 */
	public void updatePosition(Vector position) {
		this.position = position;
		pP[cDex] = position;

		// here we estimate the current velocity by comparing the past positions
		// to each other. This should reduce noise and odd values.
		if (cDex != 0) {
			velocity = velocity.add(Vector.subtract(pP[storePast - 1], pP[0]));
		}
		for (int i = 1; i < storePast; i++) {
			if (i != cDex) {
				velocity = velocity.add(Vector.subtract(pP[i - 1], pP[i]));
			}
		}
		velocity.x /= 10;
		velocity.y /= 10;

		cDex = (cDex + 1) % storePast;

	}

	/**
	 * Returns the estimated position of the object some time steps in the
	 * future. Normal negative acceleration on a surface for a rolling ball is
	 * estimated... but I don't think this is working correctly yet.
	 * 
	 * @param time
	 *            The number of timesteps in the future.
	 * @return A vector representing the position of the object the specified
	 *         time in the future.
	 */
	public Vector futurePosition(int time) {
		return new Vector(
				(float) (velocity.x *time) + position.x,
				(float) (velocity.y *time) + position.y);
	}

}
